#include "utility.h"
#include "common.h"
#include "geometry.h"

bool Utility::Project3d_2d(Eigen::Vector3d pt3d, Eigen::Vector2d &img2D, Eigen::Matrix3d K)
{
	if (pt3d.z() > 0)
	{
		Eigen::Vector3d rpt = K * pt3d;
		rpt /= rpt.z();
		img2D = Eigen::Vector2d(rpt.x(), rpt.y());
		return true;
	}
	return false;
}

std::vector<hdmap_lane> Utility::LoadByRange(const std::vector<hdmap_lane> &lanes, const GeoPoint &gps_point, double tolerance)
{
	envelope2D env(gps_point.x, gps_point.y, gps_point.x, gps_point.y);
	env.ExpandBy(tolerance);

	std::vector<hdmap_lane> find_lines;
	for (hdmap_lane lane : lanes)
	{
		double bearing = std::fabs(lane.getDir() - gps_point.bearing);
		if (bearing > M_PI / 4)
		{
			continue;
		}
		geometry::Resample(lane.geometry, 3.0);
		std::vector<Eigen::Vector3d> geometry3d;
		for (std::size_t i = 0; i < lane.geometry.size(); i++)
		{
			Eigen::Vector3d lane_pos = lane.geometry[i];
			if (env.IsInRect(lane_pos.x(), lane_pos.y()))
			{
				geometry3d.push_back(lane_pos);
			}
		}
		if (!geometry3d.empty())
		{
			lane.geometry = geometry3d;
			find_lines.push_back(lane);
		}
	}
	return find_lines;
}
